/*
 * featureDetectionOnCamera.cpp
 *
 *  Created on: 01/01/2013
 *      Author: filipe
 */

#include <vector>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>


using namespace std;
using namespace cv;


//#define __SHOW_FEATURES_IN_IMAGE__
#define __PRINT_FEATURES__

vector<string> image_filenames;
string feature_detector;

CvSeq*
detectFeaturesWithSurf (IplImage* imagem)
{
	IplImage *gray = cvCreateImage(cvSize(imagem->width, imagem->height), imagem->depth, 1);
	cvCvtColor(imagem, gray, CV_BGR2GRAY);

	CvSeq *keypoints = NULL;
	CvSeq *descriptors = NULL;
	CvMemStorage *storage = cvCreateMemStorage(0);
	CvSURFParams params = cvSURFParams(500, 1);

	cvExtractSURF(gray, NULL, &keypoints, &descriptors, storage, params, 0);
	return keypoints;
}


CvSeq*
detectFeaturesWithStar(IplImage *imagem)
{
	int i = 0;
	int maxSize = 8;
	int responseThreshold = 6;
	int lineThresholdProjected = 6;
	int lineThresholdBinarized = 6;
	int suppressNonmaxSize = 0;

	CvStarDetectorParams star_param =
		cvStarDetectorParams(
		maxSize,
		responseThreshold,
		lineThresholdProjected,
		lineThresholdBinarized,
		suppressNonmaxSize
	);

	IplImage *gray = cvCreateImage(cvSize(imagem->width, imagem->height), imagem->depth, 1);
	cvCvtColor(imagem, gray, CV_BGR2GRAY);

	CvMemStorage *storage = cvCreateMemStorage(0);
	CvSeq *keypoints = cvGetStarKeypoints(gray, storage, star_param);

	return keypoints;
}


vector<KeyPoint>
detectFeaturesWithFast(IplImage *imagem)
{
	IplImage *gray = cvCreateImage(cvSize(imagem->width, imagem->height), imagem->depth, 1);
	cvCvtColor(imagem, gray, CV_BGR2GRAY);

	Mat mat(gray);
	vector<KeyPoint> keypoints2;

	FAST(mat, keypoints2, 9, true);

	return keypoints2;
}


void
drawFeature(IplImage* image, CvPoint pt, int size, CvScalar color)
{
	CvPoint center;
	int radius;

	center.x = cvRound(pt.x);
	center.y = cvRound(pt.y);
	radius = cvRound(size * 1.2/9. * 2);

	cvCircle(image, center, radius, color, 1, 8, 0);
}


void
drawSURFFeatures(IplImage *imagem, CvSeq *keypoints)
{
	int i;

	for(i = 0; i < keypoints->total; i++)
	{
		CvSURFPoint *r = (CvSURFPoint*) cvGetSeqElem(keypoints, i);
		drawFeature(imagem, cvPoint(r-> pt.x, r-> pt.y), r->size, cvScalar(255, 0, 0, 0));
	}
}


void
drawStarFeatures(IplImage *imagem, CvSeq *keypoints)
{
	int i;

	for(i = 0; i < keypoints->total; i++)
	{
		CvStarKeypoint *r = (CvStarKeypoint*) cvGetSeqElem(keypoints, i);
		drawFeature(imagem, r->pt, r->size, cvScalar(0, 255, 0, 0));
	}
}


void
drawFASTFeatures(IplImage *imagem, vector<KeyPoint> &keypoints)
{
	int i;

	for(i = 0; i < keypoints.size(); i++)
	{
		KeyPoint k;

		drawFeature(
			imagem,
			cvPoint(keypoints[i].pt.x, keypoints[i].pt.y),
			keypoints[i].size,
			cvScalar(0, 0, 255, 0)
		);
	}
}


void
print_star_features(CvSeq *Star_keypoints)
{
	int i;

	printf("%d ", Star_keypoints->total);

	for(i = 0; i < Star_keypoints->total; i++)
	{
		CvStarKeypoint *r = (CvStarKeypoint*) cvGetSeqElem(Star_keypoints, i);
		printf("%d %d ", (int) r->pt.x, (int) r->pt.y);
	}
}


void
printf_fast_features(vector<KeyPoint> Fast_keypoints)
{
	printf("%ld ", Fast_keypoints.size());

	for (int i = 0; i < Fast_keypoints.size(); i++)
		printf("%d %d ", (int) Fast_keypoints[i].pt.x, (int) Fast_keypoints[i].pt.y);

	printf("\n");
}


void
read_image_list(char *image_list_filename)
{
	char *image_filename = (char *) calloc (1024, sizeof(char));
	FILE *file = fopen(image_list_filename, "r");

	while (!feof(file))
	{
		fscanf(file, "\n%s\n", image_filename);
		image_filenames.push_back(string(image_filename));
	}

	fclose(file);
	free(image_filename);
}


int
parse_arguments(int argc, char *argv[])
{
	if (argc < 3)
		return 0;

	read_image_list(argv[1]);
	feature_detector = string(argv[2]);

	if ((feature_detector.compare("SURF") != 0) &&
		(feature_detector.compare("FAST") != 0) &&
		(feature_detector.compare("STAR") != 0))
			return 0;

	return 1;
}


void
show_usage_information_and_exit(char *argv[])
{
	printf("\nUse %s <image-list> <detector=[SURF | STAR | FAST]>\n", argv[0]);
	printf("\n");

	exit(-1);
}


int
main(int argc, char* argv[])
{
	if (parse_arguments(argc, argv) == 0)
		show_usage_information_and_exit(argv);

	if (feature_detector.compare("SURF") == 0)
		cvNamedWindow("SURF", CV_WINDOW_AUTOSIZE);
	else if (feature_detector.compare("STAR") == 0)
		cvNamedWindow("STAR", CV_WINDOW_AUTOSIZE);
	else if (feature_detector.compare("FAST") == 0)
		cvNamedWindow("FAST", CV_WINDOW_AUTOSIZE);

	for(unsigned int i = 0; i < image_filenames.size(); i++)
	{
		IplImage *image = cvLoadImage(image_filenames[i].c_str(), CV_LOAD_IMAGE_COLOR);

		if (feature_detector.compare("SURF") == 0)
		{
			CvSeq *SURF_keypoints = detectFeaturesWithSurf(image);

#ifdef __PRINT_FEATURES__

			printf("print features not implemented for this feature...\n");

#endif

#ifdef __SHOW_FEATURES_IN_IMAGE__

			printf("num SURF features: %d\n", SURF_keypoints->total);

			IplImage *surf_keypoints_img = cvCreateImage(cvSize(image->width, image->height), image->depth, 3);
			cvCopyImage(image, surf_keypoints_img);

			drawSURFFeatures(surf_keypoints_img, SURF_keypoints);
			cvShowImage("SURF", surf_keypoints_img);
			cvWaitKey(-1);

			cvReleaseImage(&surf_keypoints_img);
#endif

		}
		else if (feature_detector.compare("STAR") == 0)
		{
			CvSeq *Star_keypoints = detectFeaturesWithStar(image);

#ifdef __PRINT_FEATURES__

			print_star_features(Star_keypoints);

#endif

#ifdef __SHOW_FEATURES_IN_IMAGE__

			printf("num STAR features: %d\n", Star_keypoints->total);

			IplImage *star_keypoints_img = cvCreateImage(cvSize(image->width, image->height), image->depth, 3);
			cvCopyImage(image, star_keypoints_img);

			drawStarFeatures(star_keypoints_img, Star_keypoints);
			cvShowImage("STAR", star_keypoints_img);
			cvWaitKey(-1);

			cvReleaseImage(&star_keypoints_img);

#endif
		}
		else if (feature_detector.compare("FAST") == 0)
		{
			vector<KeyPoint> Fast_keypoints = detectFeaturesWithFast(image);

#ifdef __PRINT_FEATURES__

			printf_fast_features(Fast_keypoints);

#endif

#ifdef __SHOW_FEATURES_IN_IMAGE__

			printf("num FAST features: %ld\n", Fast_keypoints.size());

			IplImage *fast_keypoints_img = cvCreateImage(cvSize(image->width, image->height), image->depth, 3);
			cvCopyImage(image, fast_keypoints_img);

			drawFASTFeatures(fast_keypoints_img, Fast_keypoints);
			cvShowImage("FAST", fast_keypoints_img);
			cvWaitKey(-1);

			cvReleaseImage(&fast_keypoints_img);
#endif
		}

		cvReleaseImage(&image);
	}

	return 0;
}

